from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os

ARGUMENTS = [
    DeclareLaunchArgument('use_sim_time', default_value='true',
                          choices=['true', 'false'],
                          description='Use sim time'),
    DeclareLaunchArgument('namespace', default_value='""',
                          description='Ignition model name'),
    DeclareLaunchArgument('world', default_value='empty_world',
                          description='World name')
]


def generate_launch_description():
    namespace = LaunchConfiguration('namespace')
    use_sim_time = LaunchConfiguration('use_sim_time')

    bridge_params = os.path.join(
        get_package_share_directory('agv_gazebo'),
        'config',
        'gz_bridge.yaml'
    )
    print( 'bridge_params:', bridge_params)
    # tf bridge
    gz_bridge_node = Node(
            package='ros_gz_bridge',
            executable='parameter_bridge',
            # namespace=namespace,
            name='ign_bridge',
            parameters=[ 
                {'use_sim_time': use_sim_time},
                {'config_file': bridge_params},
            ],
            output='screen'
        )
    
    image_bridge_node = Node(
        package='ros_gz_image',
        executable='image_bridge',
        # 传入所有需要特殊处理的图像话题（用空格分隔）
        arguments=['/depth_camera/image', '/depth_camera/depth_image', '/camera/image'],
        output='screen',
    )

    ld = LaunchDescription(ARGUMENTS)
    ld.add_action(image_bridge_node)
    ld.add_action(gz_bridge_node)
    return ld
